/**
 * @FilePath: \ros2\src\agv_driver\code_read_camera\code_read_camera\include\code_read_camera\code_read_camera.h
 * @Date: 2025-03-16 08:35:08
 * @Description:  QR Code Camera Reader Node
 * @Author: zhahaibo
 * @Version: 2.0.0
 * @LastEditors: haibo haibo.yang@lgmgim.cn
 * @LastEditTime: 2025-03-21 16:20:04
 * @Copyright (c) 2025 by 临工智能信息科技有限公司, All Rights Reserved. 
**/

#ifndef CODE_READ_CAMERA_H
#define CODE_READ_CAMERA_H

#include <rclcpp/rclcpp.hpp>
#include <socketcan_interface/socketcan.h>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <slam_ros_msgs/msg/qr_code.hpp>
#include <agv_msgs/msg/agv_event.hpp>
#include <std_msgs/msg/u_int64.hpp>
#include <std_msgs/msg/float64.hpp>
#include <code_read_camera_msgs/msg/qr_codeinfo_group.hpp>

#include <map>
#include <vector>
#include <thread>
#include <memory>

constexpr uint8_t NMT_OPERATION = 0x01;
constexpr uint8_t SDO_W_FUNC = 0xAA;
constexpr uint8_t SDO_R_FUNC = 0xBB;
constexpr uint16_t HEARTBEAT_CYCLE = 100;
constexpr uint8_t ROS_NODEID = 3;
constexpr uint8_t OPERATION_STATE = 5;

class CodeReadCamera : public rclcpp::Node {
public:
  struct QRCodeInfo {
    int32_t xpos;
    int32_t ypos;
    int16_t angle;
    uint32_t tag;
    uint32_t timestamp;
    double update_timestamp;
  };

  explicit CodeReadCamera();
  ~CodeReadCamera() override;

private:
  rclcpp::Publisher<code_read_camera_msgs::msg::QrCodeinfoGroup>::SharedPtr qr_code_group_pub_;
  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr landmark_pub_;
  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr shelf_landmark_pub_;
  rclcpp::Publisher<agv_msgs::msg::AGVEvent>::SharedPtr event_pub_;
  // 2023.3.20
  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr ori_landmark_pub_;
  rclcpp::Publisher<slam_ros_msgs::msg::QRCode>::SharedPtr slam_landmark_pub_;

  std::vector<rclcpp::Subscription<std_msgs::msg::UInt64>::SharedPtr> downward_pdo_subs_;
  std::vector<rclcpp::Subscription<std_msgs::msg::UInt64>::SharedPtr> upward_pdo_subs_;
  rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr downward_pdo2_ts_sub_;
  rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr upward_pdo2_ts_sub_;

  std::vector<code_read_camera_msgs::msg::QrCodeinfo> qr_code_info_vect_;
  code_read_camera_msgs::msg::QrCodeinfoGroup qr_code_info_group_;
  geometry_msgs::msg::PoseStamped ori_landMark_;
  std::map<int, QRCodeInfo> qr_code_info_;
  std::vector<int> node_ids_;
  rclcpp::Time begin_time_;
  uint8_t init_step_ = 0;
  bool use_pdo_timestamp_ = false;
  int camera_latency_ = 0;
  std::string can_device_ = "";

  std::thread processing_thread_;
  mutable std::mutex data_mutex_;

  int earth_cam_node_id_;
  int shelf_cam_node_id_;

  void initialize();
  void init_devices();
  void process_qr_data();
  bool init_device(int& node_id, const std::string& node_name);
  void setup_pdo_subscriptions(int node_id, bool is_downward);
  bool validate_landmark(const geometry_msgs::msg::PoseStamped& msg) const;
  bool validate_camera_data(const QRCodeInfo& info) const;
  
  void downward_pdo0_sub_callback(const std_msgs::msg::UInt64::ConstSharedPtr& msg);
  void downward_pdo1_sub_callback(const std_msgs::msg::UInt64::ConstSharedPtr& msg);
  void downward_pdo2_sub_callback(const std_msgs::msg::UInt64::ConstSharedPtr& msg);
  void downward_pdo3_sub_callback(const std_msgs::msg::UInt64::ConstSharedPtr& msg);
  void downward_pdo2_timestamp_sub_callback(const std_msgs::msg::Float64::ConstSharedPtr& msg);
  void upward_pdo0_sub_callback(const std_msgs::msg::UInt64::ConstSharedPtr& msg);
  void upward_pdo1_sub_callback(const std_msgs::msg::UInt64::ConstSharedPtr& msg);
  void upward_pdo2_sub_callback(const std_msgs::msg::UInt64::ConstSharedPtr& msg);
  void upward_pdo3_sub_callback(const std_msgs::msg::UInt64::ConstSharedPtr& msg);
  void upward_pdo2_timestamp_sub_callback(const std_msgs::msg::Float64::ConstSharedPtr& msg);

  void convert_to_landmark(
      const QRCodeInfo& info,
      geometry_msgs::msg::PoseStamped& landmark,
      slam_ros_msgs::msg::QRCode& slam_landmark);

  void convert_to_shelf_landmark(
      const QRCodeInfo& info,
      geometry_msgs::msg::PoseStamped& landmark);

  template<typename T>
  inline T extract_value(uint64_t data, std::initializer_list<int> shifts) const {
    T value = 0;
    int offset = 0;
    for (auto shift : shifts) {
      value |= static_cast<T>((data >> shift) & 0xFF) << (8 * (shifts.size() - 1 - offset));
      ++offset;
    }
    return value;
  }

  void publish_events();
  void setup_publishers();
  void setup_parameters();
  void start_processing_thread();
};

#endif // CODE_READ_CAMERA_H